/********************************************************************************
* Project       : No Latency RTSP streaming Project
* Desc          : video 4 linux 2
* Desc          : considered the original source(http://v4l2spec.bytesex.org/)
* Author        : Seo, SeokWon 
* e-mail        : kueony@gmail.com
* Licence       : See the Copyright
*********************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <asm/types.h>          /* for videodev2.h */
#include <linux/videodev2.h>

#include "lib_log.h"
#include "vll_v4l2.h"

#define V4L2_BUF_NUM	4
#define MSET(x) memset (&(x), 0, sizeof (x))



/*****************************************************************************/
/* Functions */
/*****************************************************************************/
static int	xioctl(int fd, int request, void* arg);
static int	open_device(st_v4l2_data* a_v4l2);
static void	close_device(st_v4l2_data* a_v4l2);
static int	init_device(st_v4l2_data* a_v4l2);


int open_v4l2(st_v4l2_data* a_v4l2, char* a_dev_name, int a_cap_width, int a_cap_height)
{
	snprintf(a_v4l2->dev_name, 127, "%s", a_dev_name);
	a_v4l2->cap_width	= a_cap_width;
	a_v4l2->cap_height	= a_cap_height;

	if(open_device(a_v4l2) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "fail_open_device");
		goto ERR_HANDLER;
	}
	
	if(init_device(a_v4l2) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "fail_init_device");
		goto ERR_HANDLER;
	}	
	
	return 0;

ERR_HANDLER:
	close_v4l2(a_v4l2);
	
	return -1;	
}

void close_v4l2(st_v4l2_data* a_v4l2)
{
	stop_capturing(a_v4l2);
	close_device(a_v4l2);
	return;
}

int stop_capturing(st_v4l2_data* a_v4l2)
{
	if(a_v4l2->fd < 0) return 0;
	
	enum v4l2_buf_type type;

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if(xioctl(a_v4l2->fd, VIDIOC_STREAMOFF, &type) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_STREAMOFF");
		return -1;
	}
	
	return 0;
}

int start_capturing(st_v4l2_data* a_v4l2)
{
	int					i;
	enum v4l2_buf_type	type;
	struct v4l2_buffer	buf;

	for (i = 0; i < a_v4l2->v4l_buf_num ; i++){
    	MSET (buf);

		buf.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory	= V4L2_MEMORY_MMAP;
		buf.index	= i;

		if(xioctl(a_v4l2->fd, VIDIOC_QBUF, &buf) < 0){
			LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_QBUF");
			return -1;
		}
	}

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	if(xioctl(a_v4l2->fd, VIDIOC_STREAMON, &type) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_STREAMON");
		return -1;
	}
	
	return 0;
}

int get_video_data(st_v4l2_data* a_v4l2, char* a_rgb, char* a_yuv)
{
	struct v4l2_buffer	buf;
	unsigned char*		yuv_data = NULL;
	int					yuv_idx, rgb_idx;
	int					r,g,b,y1,y2,u,v;
	int					idx_max = a_v4l2->cap_width * a_v4l2->cap_height;
		
	MSET(buf);

	buf.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory	= V4L2_MEMORY_MMAP;

	if(xioctl(a_v4l2->fd, VIDIOC_DQBUF, &buf) < 0) return -1;
	
	yuv_data = (unsigned char*) a_v4l2->v4l_buf[buf.index].data;
	
	memcpy(a_yuv, yuv_data, a_v4l2->v4l_buf[buf.index].length);
	
	if(a_rgb == NULL){
		if (xioctl(a_v4l2->fd, VIDIOC_QBUF, &buf) < 0) return -1;
		return 0;
	}
		
	for(rgb_idx = 0, yuv_idx = 0; rgb_idx < idx_max ; rgb_idx+=2, yuv_idx++)
	{
		/* yuv has 2 pixels per 4 bytes */
		y1	= yuv_data[yuv_idx*4+0]-16;
		u	= yuv_data[yuv_idx*4+1]-128;
		y2	= yuv_data[yuv_idx*4+2]-16;
		v	= yuv_data[yuv_idx*4+3]-128;
	
		r	= (298*y1+409*v+128) >> 8;
		g	= (298*y1-100*u-208*v+128) >> 8;
		b	= (298*y1+516*u+128) >> 8;

		if(r > 255) r = 255;
		if(g > 255) g = 255;
		if(b > 255) b = 255;
		if(r < 0) r = 0;
		if(g < 0) g = 0;
		if(b < 0) b = 0;

		a_rgb[rgb_idx*4+0] = b;
		a_rgb[rgb_idx*4+1] = g;
		a_rgb[rgb_idx*4+2] = r;

		r	= (298*y2+409*v+128) >> 8;
		g	= (298*y2-100*u-208*v+128) >> 8;
		b	= (298*y2+516*u+128) >> 8;

		if(r > 255) r = 255;
		if(g > 255) g = 255;
		if(b > 255) b = 255;
		if(r < 0) r = 0;
		if(g < 0) g = 0;
		if(b < 0) b = 0;

		a_rgb[(rgb_idx+1)*4+0] = b;
		a_rgb[(rgb_idx+1)*4+1] = g;
		a_rgb[(rgb_idx+1)*4+2] = r;
	}

	if (xioctl(a_v4l2->fd, VIDIOC_QBUF, &buf) < 0) return -1;

	return 0;
}

static int xioctl(int fd, int request, void* arg)
{
	int r;

	do r = ioctl (fd, request, arg);
	while (-1 == r && EINTR == errno);

	return r;
}

static int open_device(st_v4l2_data* a_v4l2)
{
	struct stat st; 

	if (stat(a_v4l2->dev_name, &st) < 0) {
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "cannot_identify_device");
		return -1;
	}

	if (!S_ISCHR (st.st_mode)) {
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s(%s)", __FILE__, __LINE__, "cannot_open_device", a_v4l2->dev_name);
		return -1;
	}

	if((a_v4l2->fd = open(a_v4l2->dev_name, O_RDWR | O_NONBLOCK, 0)) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s(%s)", __FILE__, __LINE__, "cannot_open_device", a_v4l2->dev_name);
		return -1;
	}

	return 0;
}

static void close_device(st_v4l2_data* a_v4l2)
{
	int i;

	for (i = 0; i < a_v4l2->v4l_buf_num ; i++)
		munmap (a_v4l2->v4l_buf[i].data, a_v4l2->v4l_buf[i].length);

	a_v4l2->v4l_buf_num = 0;

	if(a_v4l2->v4l_buf != NULL) free(a_v4l2->v4l_buf);
	a_v4l2->v4l_buf = NULL;
	
	close(a_v4l2->fd);
	a_v4l2->fd = -1;
	
	return;
}

static int init_device(st_v4l2_data* a_v4l2)
{
	struct v4l2_capability		cap;
	struct v4l2_cropcap			cropcap;
	struct v4l2_crop			crop;
	struct v4l2_format			fmt;
	struct v4l2_requestbuffers	req;
	unsigned int				min;
	unsigned int				i;

	if (xioctl(a_v4l2->fd, VIDIOC_QUERYCAP, &cap) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_QUERYCAP");
		return -1;
	}

	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "this_is_not_video_capture_device");
		return -1;
	}

	if (!(cap.capabilities & V4L2_CAP_STREAMING)){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "this_does_not_support_streaming_io");
		return -1;
	}

	MSET (cropcap);
	
	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	if (xioctl(a_v4l2->fd, VIDIOC_CROPCAP, &cropcap) == 0){
		crop.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
		crop.c		= cropcap.defrect; /* reset to default */
		xioctl (a_v4l2->fd, VIDIOC_S_CROP, &crop);
	}

	MSET (fmt);

	fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width       = a_v4l2->cap_width; 
	fmt.fmt.pix.height      = a_v4l2->cap_height;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
	fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

	if(xioctl (a_v4l2->fd, VIDIOC_S_FMT, &fmt) < 0){
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_S_FMT");
		return -1;
	}

	/* Buggy driver paranoia. */
	min = fmt.fmt.pix.width * 2;

	if (fmt.fmt.pix.bytesperline < min) fmt.fmt.pix.bytesperline = min;
	
	min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;

	if (fmt.fmt.pix.sizeimage < min) fmt.fmt.pix.sizeimage = min;
	
	a_v4l2->cap_byteperline = fmt.fmt.pix.bytesperline;
	a_v4l2->cap_data_size	= fmt.fmt.pix.sizeimage;
		
    MSET (req);
    req.count	= V4L2_BUF_NUM;
    req.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory	= V4L2_MEMORY_MMAP;

	if(xioctl(a_v4l2->fd, VIDIOC_REQBUFS, &req) < 0) {
		if (EINVAL == errno) {
			LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "this_does_not_support_memory_mapping");
			return -1;
		} else {
			LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_REQBUFS");
			return -1;
		}
	}

	if (req.count < 2) {
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "device_allows_insufficient_buf");
		return -1;
	}
	
	a_v4l2->v4l_buf	= (st_v4l2_buf*) calloc (req.count, sizeof(st_v4l2_buf));
	
	if (a_v4l2->v4l_buf == NULL) {
		LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "cannot_allocate_memory");
		return -1;
	}

	for (i = 0; i < req.count; i++) {
		struct v4l2_buffer buf;

		MSET (buf);

		buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory      = V4L2_MEMORY_MMAP;
		buf.index       = i;
	
		if (xioctl(a_v4l2->fd, VIDIOC_QUERYBUF, &buf) < 0){
			LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "VIDIOC_QUERYBUF");
			return -1;
		}

		// YUV: 2bytes per pixels = width * height * 2
		a_v4l2->v4l_buf[i].length	= buf.length;
		a_v4l2->v4l_buf[i].data		= mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, a_v4l2->fd, buf.m.offset);

		if(MAP_FAILED == a_v4l2->v4l_buf[i].data){
			LogMsg(LIB_LOG_TYPE_ERROR, "(%s, %d) %s", __FILE__, __LINE__, "mmap");
			return -1;		
		}
		
		a_v4l2->v4l_buf_num = (i+1);
	}
	
	return 0;
}
